#include "cpuport.h"
#include "hwutil.h"
#include "io.h"
#include "printk.h"

struct exception_stack_frame
{
    uint32_t r0;
    uint32_t r1;
    uint32_t r2;
	uint32_t r3;
	uint32_t r12;
	uint32_t lr;
	uint32_t pc;
	uint32_t psr;
};

struct stack_frame
{
	uint32_t r4;
	uint32_t r5;
	uint32_t r6;
	uint32_t r7;
	uint32_t r8;
	uint32_t r9;
	uint32_t r10;
	uint32_t r11;
    
    struct exception_stack_frame exception_stack_frame;
};

struct exc_frame
{
    uint32_t exc_return;
  
	uint32_t r4;
	uint32_t r5;
	uint32_t r6;
	uint32_t r7;
	uint32_t r8;
	uint32_t r9;
	uint32_t r10;
	uint32_t r11;
    
    struct exception_stack_frame exception_stack_frame;
};

NO_INIT uint32_t task_switch_interrupt_flag KERNEL_SECTION;
NO_INIT uint32_t interrupt_from_task KERNEL_SECTION;
NO_INIT uint32_t interrupt_to_task KERNEL_SECTION;

void cpu_track_report(struct exc_frame *frame)
{
  extern void tasksched_status_report(void);
  
  printk("\r\nHard default in %s mode.\r\n", (frame->exc_return & (1 << 2)) ? ("process") : ("irq"));
	
  printk("R0  is 0x%08x .\r\n", frame->exception_stack_frame.r0);
  printk("R1  is 0x%08x .\r\n", frame->exception_stack_frame.r1);
  printk("R2  is 0x%08x .\r\n", frame->exception_stack_frame.r2);
  printk("R3  is 0x%08x .\r\n", frame->exception_stack_frame.r3);
	
  printk("R4  is 0x%08x .\r\n", frame->r4);
  printk("R5  is 0x%08x .\r\n", frame->r5);
  printk("R6  is 0x%08x .\r\n", frame->r6);
  printk("R7  is 0x%08x .\r\n", frame->r7);
  printk("R8  is 0x%08x .\r\n", frame->r8);
  printk("R9  is 0x%08x .\r\n", frame->r9);
  printk("R10 is 0x%08x .\r\n", frame->r10);
  printk("R11 is 0x%08x .\r\n", frame->r11);
	
  printk("R12 is 0x%08x .\r\n", frame->exception_stack_frame.r12);
  printk("LR  is 0x%08x .\r\n", frame->exception_stack_frame.lr);
  printk("PC  is 0x%08x .\r\n", frame->exception_stack_frame.pc);
  printk("PSR is 0x%08x .\r\n", frame->exception_stack_frame.psr);

  tasksched_status_report();

  while(1);
}

char* task_hw_stack_init(void entry(void *),
			             void *paramter,
						 char *stack,
			             phys_addr_t size,
			             void exit(void *))
{
    struct stack_frame *stack_frame;
    char *stk = stack + size;
  
    stk = stack + size;
    stk = (char *)__ALIGN(stk, 8);
    
    stk = stk - sizeof(struct stack_frame);
    stack_frame = (struct stack_frame *)stk;
    
    for(int i = 0; i < sizeof(struct stack_frame) / sizeof(uint32_t); i++)
    {  
        ((int*)stack_frame)[i] = 0xdeadbeef;
    }
    
    stack_frame->exception_stack_frame.r0  = (uint32_t)paramter;
    stack_frame->exception_stack_frame.r1  = 0;
    stack_frame->exception_stack_frame.r2  = 0;
    stack_frame->exception_stack_frame.r3  = 0;
    stack_frame->exception_stack_frame.r12 = 0;
    stack_frame->exception_stack_frame.lr  = (int)exit;
    stack_frame->exception_stack_frame.pc  = (int)entry;
    stack_frame->exception_stack_frame.psr = 0x01000000L;
    
    return stk;  
}

